/***************************************************************************
 *  Filename: parrothandler.cpp
 *  Class: ParrotHandler
 *
 *  Copyright 2012  Jacco van der Spek
 *  <jaccovdspek@gmail.com>
 *
 *  Copyright 2012  Mario Henrique Voorsluys
 *  <forcaeluz@gmail.com>
 *
 ****************************************************************************/

/*
 *
 * This file is part of %ProjectName%.
 *
 *    %ProjectName% is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 3 of the License, or
 *    (at your option) any later version.
 *
 *    %ProjectName% is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with %ProjectName%.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "parrothandler.h"


ParrotHandler::ParrotHandler(QString port, QObject *parent):QObject(parent),communicator(true, parent)
{
    communicator.setupPort(port, BAUD19200, DATA_8, PAR_NONE, FLOW_OFF, STOP_1);
    connect(&communicator, SIGNAL(byteAvailable()), this, SLOT(readData()));

}

void ParrotHandler::readData(){

    buffer.append(communicator.receiveRawFromPort());
    char headerchars[4] = {0xFE, 0XFE, 0XFE, 0XFE};
    int begin = 0;
    int loc = buffer.indexOf(headerchars);
    if(loc==-1){
        qDebug() << "[ParrotHandler] No header found";
        return;
    }
    float x;
    float y;
    float z;
    int zi;
    quint8 tag;
    int size;

    buffer = buffer.mid(loc);
    QByteArray tmp;
    begin += 4;
    tmp = buffer.mid(begin,1);
    begin += 1;
    tag = *reinterpret_cast<quint8*>(tmp.data());
    tmp = buffer.mid(begin, 1);
    begin += 1;
    size = *reinterpret_cast<quint8*>(tmp.data());
    if(buffer.size() < size){
        qDebug() << "[ParrotHandler] Message not ready yet";
        return;
    }
    switch(tag){
    case DATA_GYR:
        qDebug() << "[ParrotHandler] Working on Gyroscope data.";
        tmp = buffer.mid(begin, 4);
        begin += 4;
        x = *reinterpret_cast<float*>(tmp.data());
        tmp = buffer.mid(begin, 4);
        begin += 4;
        y = *reinterpret_cast<float*>(tmp.data());
        tmp = buffer.mid(begin, 4);
        begin += 4;
        z = *reinterpret_cast<float*>(tmp.data());
        emit(receivedAngles(x, y, z));
        qDebug() << "[ParrotHandler] Current angles are: " << x << "," << y << "," << z;
        break;
    case DATA_VEL:
        qDebug() << "[ParrotHandler] Working on speed data.";
        tmp = buffer.mid(begin, 4);
        begin += 4;
        x = *reinterpret_cast<float*>(tmp.data());
        tmp = buffer.mid(begin, 4);
        begin += 4;
        y = *reinterpret_cast<float*>(tmp.data());
        tmp = buffer.mid(begin, 4);
        begin += 4;
        z = *reinterpret_cast<float*>(tmp.data());
        emit(receivedVelocities(x, y, z));
        qDebug() << "[ParrotHandler] Current velocities are: " << x << "," << y << "," << z;
        break;
    case DATA_BAT:
        qDebug() << "[ParrotHandler] Working on battery data.";
        tmp = buffer.mid(begin, 4);
        begin += 4;
        zi = *reinterpret_cast<int*>(tmp.data());
        emit receivedBattery(zi);
        break;
    case DATA_ALTITUDE:
        qDebug() << "[ParrotHandler] Working on altitude data.";
        tmp = buffer.mid(begin, 4);
        begin += 4;
        zi = *reinterpret_cast<int*>(tmp.data());
        emit receivedBattery(zi);
        qDebug() << "[ParrotHandler] Altitude is: " << zi;
        break;
    default:
        qDebug() << "[ParrotHandler] Message not understood" << tmp.toHex();
    }
    buffer = buffer.mid(begin);
}

void ParrotHandler::sendTakeOff(){
    quint32 header = HEADER;
    quint8 tag = COMMAND_TAKEOFF;
    quint8 size = 6;
    QByteArray data;
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&header)), 4));
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&tag)), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&size), 1));
    sendData(data);
}

void ParrotHandler::sendLand(){
    quint32 header = HEADER;
    quint8 tag = COMMAND_LAND;
    quint8 size = 6;
    QByteArray data;
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&header)), 4));
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&tag)), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&size), 1));
    sendData(data);
}

void ParrotHandler::sendEmergency(){
    quint32 header = HEADER;
    quint8 tag = COMMAND_EMERG;
    quint8 size = 6;
    QByteArray data;
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&header)), 4));
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&tag)), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&size), 1));
    sendData(data);
}

void ParrotHandler::sendReset(){
    quint32 header = HEADER;
    quint8 tag = COMMAND_SYNC;
    quint8 size = 6;
    QByteArray data;
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&header)), 4));
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&tag)), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&size), 1));
    sendData(data);
}

void ParrotHandler::sendRelativeTarget(float x, float y, float z){
    quint32 header = HEADER;
    quint8 tag = COMMAND_TARGET;
    quint8 size = 18;
    QByteArray data;
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&header)), 4));
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&tag)), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&size), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&x), 4));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&y), 4));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&z), 4));
    sendData(data);
}

void ParrotHandler::sendGPSTarget(float lat, float lng, float altitude){
    quint32 header = HEADER;
    quint8 tag = COMMAND_TARGET;
    quint8 size = 18;
    QByteArray data;
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&header)), 4));
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&tag)), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&size), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&lat), 4));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&lng), 4));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&altitude), 4));
    sendData(data);
}

void ParrotHandler::sendData(QByteArray data){
    communicator.sendToPort(data);
    qDebug() << "[ParrotHandler] Data sent to parrot: " << data.toHex();
}
